#include "Com_PID.h"

void Com_PID_Calculate(PID_Struct *pid, float dt)
{
    // 1.当前误差
    float error = pid->measure - pid->desire;

    // 2.误差积分
    pid->integral = pid->integral + error * dt;

    // 3.误差微分
    float derivative = (error - pid->last_error) / dt;
    pid->last_error  = error;

    pid->output = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;
}

void Com_PID_Cascade(PID_Struct *outer, PID_Struct *inner, float dt)
{
    Com_PID_Calculate(outer, dt);
    inner->desire = outer->output;
    Com_PID_Calculate(inner, dt);
}